/*************************************************** 
  This is an example for the Adafruit Triple-Axis Gyro sensor

  Designed specifically to work with the Adafruit L3GD20 Breakout 
  ----> https://www.adafruit.com/products/1032

  These sensors use I2C or SPI to communicate, 2 pins (I2C) 
  or 4 pins (SPI) are required to interface.

  Adafruit invests time and resources providing this open source code, 
  please support Adafruit and open-source hardware by purchasing 
  products from Adafruit!

  Written by Kevin "KTOWN" Townsend for Adafruit Industries.  
  BSD license, all text above must be included in any redistribution
 ****************************************************/

#include <Wire.h> 
#include <Adafruit_L3GD20.h>
#include <LSM303.h>

// Instantiate a gyro object
Adafruit_L3GD20 gyro;
// Instantiate a compass/accel object
LSM303 compass;

void setup() 
{
  Serial.begin(230400);
  
  // Initialize the gyro
  if (!gyro.begin(gyro.L3DS20_RANGE_2000DPS))
  {
    Serial.println("Oops ... unable to initialize the L3GD20. Check your wiring!");
    while (1);
  }
  gyro.write8(gyro.L3GD20_REGISTER_CTRL_REG1, 0b10011111);
  
  // Compass setup
  Wire.begin();
  compass.init(); //Turns on LSM303DLH
  compass.writeAccReg(LSM303_CTRL_REG1_A, 0x3F); // Acc output data rate (ODR) = 1000 Hz
  compass.writeAccReg(LSM303_CTRL_REG4_A, 0x30); // Set accelerometer scale to +/-8 g
  compass.writeMagReg(LSM303_MR_REG_M, 0x00);    // Default
  compass.writeMagReg(LSM303_CRA_REG_M, 0x18);   // Mag output data rate (ODR) = 75 Hz (max)
  delay(1000);
  Serial.print("Time,GyroX,GyroY,GyroZ,CompX,CompY,CompZ,MagX,MagY,MagZ");
}

void loop() 
{ 
  gyro.read();
  compass.read();
  Serial.print(millis());
  Serial.print(",");
  Serial.print((int)gyro.data.x);
  Serial.print(",");
  Serial.print((int)gyro.data.y);
  Serial.print(",");
  Serial.print((int)gyro.data.z);
  Serial.print(",");
  Serial.print((int)compass.a.x);
  Serial.print(",");
  Serial.print((int)compass.a.y);
  Serial.print(",");
  Serial.print((int)compass.a.z);
  Serial.print(",");
  Serial.print((int)compass.m.x);
  Serial.print(",");
  Serial.print((int)compass.m.y);
  Serial.print(",");
  Serial.println((int)compass.m.z);

}
